/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : pwm.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "pwm.h"
#include "pipo.h"
#include "i2cmaster.h"
#include "eeprom.h"

int16_t		pwm_in_val[NUMPPM_IN] = {0};
int16_t		pwm_in_delta[NUMPPM_IN] = {0};
uint16_t	ch_out_val[NUMPPM_OUT]; //var to hold the outgoing values


void pwm_out_set(uint8_t idx, int16_t v)
{
    //outputting real values to ch_out_val to increase speed
    //limitdata holds values in increments of 1 uS.   The count is in 0.5 uS
    //values in ch_out_val end up to be in 0.5 uS increments
    int16_t hspan = eeData.limitData[idx].max - eeData.limitData[idx].min;  //not dividing by 2 so we convert to 0.5uS

    //v is already in 0.5uS increments
    if(v<(-hspan))
        ch_out_val[idx] = eeData.limitData[idx].min << 1;
    else if(v>(hspan))
        ch_out_val[idx] = eeData.limitData[idx].max << 1;
    else
        ch_out_val[idx] = v + eeData.limitData[idx].max + eeData.limitData[idx].min; //not dividing by 2 -> convert to 0.5uS
}


int16_t pwmIn(uint8_t ch)
{
    return pwm_in_val[ch];
}


/*
 * pwm in reader
 * interrupt fires whenever one of the PWM_IN pins change, NOBLOCK so pwm out can run uninterrupted
 */
ISR(PCINT2_vect)  // blocking so we need to enable ASAP
{
    static uint16_t dt,dtt;  //using static to prevent push/pop every time ISR is called
    static uint8_t  pwm_in_state, chng;
    static uint16_t pwm_in_timer[NUMPPM_IN];

    dtt=TCNT1; //read current timer to dt;

    sei();
    chng = pwm_in_state ^ PWM_IN_MASK;
    pwm_in_state = PWM_IN_MASK;

    for(uint8_t i=0; i<NUMPPM_IN; i++)
        if(chng & (1<<i))
        {
            dt = dtt-pwm_in_timer[i]; //dt now holds the delta of the current timing
            pwm_in_timer[i] = dtt; //pwm_in_timer is set to current time
            if ((dt>PWM_STICK_MIN) && (dt<PWM_STICK_MAX))  // allows for radio outputting upto 125%
            {  // 2000 ticks per 1msec.   (1.5*2000 +-1024) -> valid value  this way we don't jave to worry about polarity

                if (dt>PWM_MAX) dt=PWM_MAX;	if (dt<PWM_MIN) dt=PWM_MIN; // scale back to max +/- 100%
                pwm_in_delta[i] = -pwm_in_val[i];
                pwm_in_val[i]	=  dt-PWM_MIDFRAME; // servo value = time-3000
                // ### TODO ### need to look at how accurate this routine is i.e. is can be interrupted
                // and truncate the val as appropriate so that stored val is steady rather than bouncing around
                pwm_in_delta[i] += pwm_in_val[i]; //pwm_in_delta = new pwm_in_val - last pwm_in_val
            }
        }
}


//ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect));  // declare pcint1 same as pcint0



/*
* PPM Out routines
*/


//outputs one PWM pulse - relies on outside timing!
void outputPWM()
{
    uint16_t startFrame;
    uint16_t deltatime = 0;

    //all outputs are high
    PORTB |= 0x3F;
    PORTC |= 0x03;

    cli();
    startFrame = TCNT1; //record start time
    sei();

    while(deltatime<maxPWMPeriod) // cap top of the loop
    {
        cli();
        deltatime = TCNT1;
        sei();
        deltatime -= startFrame;

        if(deltatime>ch_out_val[7]) OUTPUT8_OFF;
        if(deltatime>ch_out_val[6]) OUTPUT7_OFF;
        if(deltatime>ch_out_val[5]) OUTPUT6_OFF;
        if(deltatime>ch_out_val[4]) OUTPUT5_OFF;
        if(deltatime>ch_out_val[3]) OUTPUT4_OFF;
        if(deltatime>ch_out_val[2]) OUTPUT3_OFF;
        if(deltatime>ch_out_val[1]) OUTPUT2_OFF;
        if(deltatime>ch_out_val[0]) OUTPUT1_OFF;
    }

    // make sure all outputs are low
    PORTB &= ~0x3F;
    PORTC &= ~0x03;
}


